#include <stdio.h>
#include <unistd.h>
#include <signal.h>
#include <stdint.h>
#include <stdlib.h>
#include <sys/select.h>
#include <time.h>
#include <string.h>
#include <stdatomic.h>
#include <fcntl.h>
#include <termios.h>

#include "sample_comm.h"
#include "color_detect.h"
#include "color_converter.h"
#include "solve.h"

// 图像尺寸
const int WIDTH = 1280;
const int HEIGHT = 720;

// 串口通信
// linux c编程,初始化ttyS1串口,将波特率设置为115200,并且实现一个
// uart_getchar_with_timeout(char *out)函数,设置超时时间为1秒,读取数据成功返回1,超时返回0.
#define UART_DEVICE "/dev/ttyS1"
#define BAUDRATE B115200
#define UART_TIMEOUT 1
static int uart_init()
{
    int fd = open(UART_DEVICE, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd < 0) {
        perror("Failed to open the uart");
        return -1;
    }
    //tcgetattr(fd, &oldtio); /* save current serial port settings */
	struct termios newtio;
    bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */
    /* Setting baud rate */
    cfsetispeed(&newtio, BAUDRATE);
    cfsetospeed(&newtio, BAUDRATE);
    /* 8N1 configuration */
    newtio.c_cflag &= ~PARENB;
    newtio.c_cflag &= ~CSTOPB;
    newtio.c_cflag &= ~CSIZE;
    newtio.c_cflag |= CS8;
    /* Disable hardware flow control */
    newtio.c_cflag &= ~CRTSCTS;
    /* Enable receiver and set local mode */
    newtio.c_cflag |= (CLOCAL | CREAD);
    /* Setup for non-canonical mode */
    newtio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    /* Read at most 1 character at a time */
    newtio.c_cc[VMIN] = 1;
    newtio.c_cc[VTIME] = 0; // No timeout here because we'll handle it in our custom function
    /* Flush Port before applying the settings */
    tcflush(fd, TCIFLUSH);
    /* Finally apply the settings */
    if (tcsetattr(fd, TCSANOW, &newtio) != 0) {
        perror("Error setting terminal attributes");
        close(fd);
        return -1;
    }
    return fd;
}
static int uart_getchar_with_timeout(int uart_fd, char *out)
{
    fd_set rfds;
    struct timeval tv;

    FD_ZERO(&rfds);
    FD_SET(uart_fd, &rfds);

    tv.tv_sec = UART_TIMEOUT;
    tv.tv_usec = 0;

    int retval = select(uart_fd + 1, &rfds, NULL, NULL, &tv);
    if (retval > 0) {
        if (read(uart_fd, out, 1) == 1) {
            return 1; // Read successful
        }
    } else if (retval == 0) {
        return 0; // Timeout
    } else {
        perror("Error in select()");
        return -1; // Error occurred
    }

    return -1; // This should not happen normally
}

// 带有超时等待的getchar
#define TIMEOUT_SECONDS 1 // 设置超时时间为1秒
static int getchar_with_timeout(char *out)
{
    struct timeval tv;
    fd_set readfds;
    FD_ZERO(&readfds);
    FD_SET(STDIN_FILENO, &readfds); // 标准输入文件描述符（通常是0）
    tv.tv_sec = TIMEOUT_SECONDS;
    tv.tv_usec = 0;
    if (select(STDIN_FILENO + 1, &readfds, NULL, NULL, &tv) == -1)
    {
        perror("select");
        return -1; // 发生错误
    }
    if (FD_ISSET(STDIN_FILENO, &readfds)) // 如果有数据可读
    {
        *out = getchar();
		return 1;
    }
    else
    {
        return 0; // 超时未收到任何输入
    }
}

// 处理Ctrl+c
_Atomic int exit_flag = 0;
void fun_sig(int sig)
{
    printf("sig = %d\n",sig);
	atomic_store(&exit_flag, 1);

}

// 复制yuv图像
// 参考SAMPLE_COMM_FRAME_SaveToFile修改
static int copy_yuv(uint8_t *buffer, VIDEO_FRAME_INFO_S *pstVideoFrame)
{
	// NV21的一个像素块由一个Y分量、一个U分量和一个V分量组成
    // 对于1280x720分辨率，宽度为1280，高度为720
    // NV21的总像素数 = 宽 * 高
    // NV21的Y分量数量 = 宽 * 高
    // UV分量数量 = (宽 / 2) * (高 / 2)
	// 检查是否是PIXEL_FORMAT_NV21
	if(pstVideoFrame->stVFrame.enPixelFormat != PIXEL_FORMAT_NV21)
	{
		printf("stVideoFrame.stVFrame.enPixelFormat=%d, need PIXEL_FORMAT_NV21.\n", 
		        pstVideoFrame->stVFrame.enPixelFormat);
		return -1;
	}
	if(WIDTH != pstVideoFrame->stVFrame.u32Stride[0] || HEIGHT != pstVideoFrame->stVFrame.u32Height)
	{
		printf("image size is not 1280x720.\n");
		return -2;
	}
	// 获取数据
	for (int i = 0; i < 2; ++i)
	{
		pstVideoFrame->stVFrame.pu8VirAddr[i]
			= CVI_SYS_Mmap(pstVideoFrame->stVFrame.u64PhyAddr[i], pstVideoFrame->stVFrame.u32Length[i]);

		printf("plane(%d): paddr(%ld) vaddr(%p) stride(%d)\n",
			   i, pstVideoFrame->stVFrame.u64PhyAddr[i],
			   pstVideoFrame->stVFrame.pu8VirAddr[i],
			   pstVideoFrame->stVFrame.u32Stride[i]);
		printf("plane_len(%d)\n", pstVideoFrame->stVFrame.u32Length[i]);
	}
	memcpy(buffer, pstVideoFrame->stVFrame.pu8VirAddr[0], pstVideoFrame->stVFrame.u32Length[0]);
	memcpy(buffer + pstVideoFrame->stVFrame.u32Length[0], 
	       pstVideoFrame->stVFrame.pu8VirAddr[1], pstVideoFrame->stVFrame.u32Length[1]);
	// 释放内存
	for (int i = 0; i < 2; ++i)
	{
		CVI_SYS_Munmap(pstVideoFrame->stVFrame.pu8VirAddr[i], pstVideoFrame->stVFrame.u32Length[i]);
	}
	return 0;
}
// 识别色块
static int get_face_color(uint8_t *cube_face_mask, uint8_t *buffer, uint8_t rgb_temp[][3])
{
	if(cube_face_mask == 0)
	{
		return -1;
	}

	int yuv_count[27][4]={0};
    for (int y = 0; y < HEIGHT; ++y) {
        for (int x = 0; x < WIDTH; ++x) 
		{
            int y_index = y * WIDTH + x;
            // 统计颜色
            int mask = cube_face_mask[y_index];
            if(mask < 27)
            {
				// 获取数据
				uint8_t *y_plane = buffer;
   		 		uint8_t *uv_plane = buffer + WIDTH*HEIGHT;
				// 计算对应像素在YUV各平面的位置
				int uv_index = ((y / 2) * (WIDTH / 2) + (x / 2)) * 2;
				// NV21：YYYYYYYYVUVU
				uint8_t y_value = y_plane[y_index];
				uint8_t v_value = uv_plane[uv_index];
				uint8_t u_value = uv_plane[uv_index + 1];
                yuv_count[mask][0] += y_value;
                yuv_count[mask][1] += u_value;
                yuv_count[mask][2] += v_value;
                yuv_count[mask][3] += 1;
            }

        }
    }
	for(int i=0; i<27; i++)
    {
        int count = yuv_count[i][3];
        uint8_t y_avg, u_avg, v_avg;
        y_avg = yuv_count[i][0] / count;
        u_avg = yuv_count[i][1] / count;
        v_avg = yuv_count[i][2] / count;
        yuv2rgb(y_avg, u_avg, v_avg, &rgb_temp[i][0], &rgb_temp[i][1], &rgb_temp[i][2]);
        //printf("i=%d, r=%d, g=%d, b=%d\n", i, rgb_temp[i][0], rgb_temp[i][1], rgb_temp[i][2]);
    }
	return 0;
}

// 保存为bmp文件
// BMP文件头结构体定义
// 共14字节
#pragma pack(push)
#pragma pack(1)
typedef struct {
    uint16_t bfType;      // 文件类型标志，固定为"BM"
    uint32_t bfSize;      // 文件大小
    uint16_t bfReserved1; // 保留字，无意义
    uint16_t bfReserved2; // 保留字，无意义
    uint32_t bfOffBits;   // 数据起始位置偏移量
} BITMAPFILEHEADER;

// BMP信息头结构体定义
// 共40字节
typedef struct {
    uint32_t biSize;          // 信息头大小
    int32_t  biWidth;         // 图像宽度
    int32_t  biHeight;        // 图像高度
    uint16_t biPlanes;        // 颜色平面数，一般为1
    uint16_t biBitCount;      // 每像素位数，此处为24
    uint32_t biCompression;   // 压缩方式，0表示不压缩
    uint32_t biSizeImage;     // 图像数据大小
    int32_t  biXPelsPerMeter; // 水平分辨率，可设为0
    int32_t  biYPelsPerMeter; // 垂直分辨率，可设为0
    uint32_t biClrUsed;       // 使用的颜色数，对于24位图设为0
    uint32_t biClrImportant;  // 重要颜色数，设为0
} BITMAPINFOHEADER;
#pragma pack(pop)
static int save_bmp(const char *out_file_name,  uint8_t *buffer)
{
	// 获取数据
	uint8_t *y_plane = buffer;
    uint8_t *uv_plane = buffer + WIDTH*HEIGHT;
	// 写入BMP
    const int padding = (4 - (WIDTH * 3) % 4) % 4; // 行对齐所填充的字节数
    const int rowSize = WIDTH * 3 + padding; // 一行实际占用字节数

    // 计算文件大小和其他需要的信息头数据
    BITMAPFILEHEADER fileHeader;
    BITMAPINFOHEADER infoHeader;

    fileHeader.bfType = 0x4D42; // 将字符'B'和'M'转换为小端序存储
    fileHeader.bfSize = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + HEIGHT * rowSize;
    fileHeader.bfReserved1 = 0;
    fileHeader.bfReserved2 = 0;
    fileHeader.bfOffBits = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER);

    infoHeader.biSize = sizeof(BITMAPINFOHEADER);
    infoHeader.biWidth = WIDTH;
    infoHeader.biHeight = -HEIGHT; // 负数表示从底向上存储
    infoHeader.biPlanes = 1;
    infoHeader.biBitCount = 24;
    infoHeader.biCompression = 0; // BI_RGB，表示无压缩
    infoHeader.biSizeImage = HEIGHT * rowSize;
    infoHeader.biXPelsPerMeter = 0;
    infoHeader.biYPelsPerMeter = 0;
    infoHeader.biClrUsed = 0;
    infoHeader.biClrImportant = 0;
    // 打开文件用于写入
    FILE* bmpFile = fopen(out_file_name, "wb");
    if (!bmpFile) {
        printf("Failed to open %s file for writing.\n", out_file_name);
        return -3;
    }
    // 写入文件头
    fwrite(&fileHeader, sizeof(fileHeader), 1, bmpFile);
    fwrite(&infoHeader, sizeof(infoHeader), 1, bmpFile);
    // 循环写入每行
    for (int y = 0; y < HEIGHT; ++y) {
		uint8_t bgr_date[3 * WIDTH];
        for (int x = 0; x < WIDTH; ++x) {
            // 计算对应像素在YUV各平面的位置
            int y_index = y * WIDTH + x;
            int uv_index = ((y / 2) * (WIDTH / 2) + (x / 2)) * 2;
            // NV21：YYYYYYYYVUVU
            uint8_t y_value = y_plane[y_index];
            uint8_t v_value = uv_plane[uv_index];
            uint8_t u_value = uv_plane[uv_index + 1];
            // 保存数据
            // 转换为RGB
            uint8_t r,g,b;
            yuv2rgb(y_value, u_value, v_value, &r, &g, &b);
            
            bgr_date[x * 3 + 2] = r;
            bgr_date[x * 3 + 1] = g;
            bgr_date[x * 3 + 0] = b;
        }
		// 写入像素
        fwrite(bgr_date, 1, 3 * WIDTH, bmpFile);
    }
    fclose(bmpFile);
    printf("wrote bitmap to %s.\n", out_file_name);
 
	return 0;
}


static int read_cube_face_mask(const char *filename, uint8_t *buff)
{
	FILE* f = fopen(filename, "rb");
	if (!f) {
		printf("Failed to open %s for read.\n", filename);
		return 1;
	}
	fread(buff, 1, WIDTH * HEIGHT, f);
	fclose(f);
	return 0;
}


int main(int argc, char *argv[])
{
	printf("cube robot init...\n");
	// 处理输入参数
	int uart_fd = -1;
	for(int i=1; i<argc; i++)
	{
		if(strcmp("--uart", argv[i]) == 0)
		{
			printf("use cmd from uart.");
			uart_fd = uart_init();
		}
	}
	

	uint8_t *buffer0 = (uint8_t *)malloc(1280*720/2*3);
	uint8_t *buffer1 = (uint8_t *)malloc(1280*720/2*3);
	uint8_t *cube_face_mask = (uint8_t *)malloc(1280*720);
	uint8_t rgb_color[54][3] = {0};
	
	// 初始化MMF
	MMF_VERSION_S stVersion;
	SAMPLE_INI_CFG_S	   stIniCfg = {0};
	SAMPLE_VI_CONFIG_S stViConfig;

	PIC_SIZE_E enPicSize;
	SIZE_S stSize;
	CVI_S32 s32Ret = CVI_SUCCESS;
	
	// 后续通过SAMPLE_COMM_VI_ParseIni，加载GC4653传感器的参数
	stIniCfg = (SAMPLE_INI_CFG_S) {
		.enSource  = VI_PIPE_FRAME_SOURCE_DEV,
		.devNum    = 2,
		.enSnsType[0] = SONY_IMX327_2L_MIPI_2M_30FPS_12BIT,
		.enWDRMode[0] = WDR_MODE_NONE,
		.s32BusId[0]  = 3,
		.MipiDev[0]   = 0xff,
		.enSnsType[1] = SONY_IMX327_SLAVE_MIPI_2M_30FPS_12BIT,
		.s32BusId[1] = 0,
		.MipiDev[1] = 0xff,
	};
	// 获取当前 MMF 软件的版本号
	CVI_SYS_GetVersion(&stVersion);
	printf("MMF Version:%s\n", stVersion.version);
	// 设置日志等级
	//LOG_LEVEL_CONF_S log_conf;
	//log_conf.enModId = CVI_ID_LOG;
	//log_conf.s32Level = CVI_DBG_ERR; // CVI_DBG_INFO
	//CVI_LOG_SetLevelConf(&log_conf);

	// Get config from ini if found.
	// 配置文件位于/mnt/data/sensor_cfg.ini，型号GC4653
	if (SAMPLE_COMM_VI_ParseIni(&stIniCfg)) {
		printf("Parse complete\n");
	}
	// Set sensor number
	CVI_VI_SetDevNum(stIniCfg.devNum);

	/************************************************
	 * step1:  Config VI
	 ************************************************/
	s32Ret = SAMPLE_COMM_VI_IniToViCfg(&stIniCfg, &stViConfig);
	if (s32Ret != CVI_SUCCESS)
		return s32Ret;

	/************************************************
	 * step2:  Get input size
	 ************************************************/
	s32Ret = SAMPLE_COMM_VI_GetSizeBySensor(stIniCfg.enSnsType[0], &enPicSize);
	if (s32Ret != CVI_SUCCESS) {
		printf("SAMPLE_COMM_VI_GetSizeBySensor failed with %#x\n", s32Ret);
		return s32Ret;
	}
	s32Ret = SAMPLE_COMM_SYS_GetPicSize(enPicSize, &stSize);
	if (s32Ret != CVI_SUCCESS) {
		printf("SAMPLE_COMM_SYS_GetPicSize failed with %#x\n", s32Ret);
		return s32Ret;
	}

	/************************************************
	 * step3:  Init modules
	 * SAMPLE_PLAT_SYS_INIT对应的调试信息：
	 * [SAMPLE_PLAT_SYS_INIT]-72: common pool[0] BlkSize 5652480
	 ************************************************/
	s32Ret = SAMPLE_PLAT_SYS_INIT(stSize);
	if (s32Ret != CVI_SUCCESS) {
		printf("sys init failed. s32Ret: 0x%x !\n", s32Ret);
		return s32Ret;
	}
	/************************************************
	 * SAMPLE_PLAT_VI_INIT对应的调试信息：
	 * ISP Vipipe(0) Allocate pa(0x8de20000) va(0x0x3fe9449000) size(291424)
	 * stSnsrMode.u16Width 2560 stSnsrMode.u16Height 1440 25.000000 wdrMode 0 pstSnsObj 0xe3ad0
	 * ...
	 * ...
	 * [SAMPLE_COMM_ISP_Thread]-95: ISP Dev 0 running!
	 ************************************************/
	s32Ret = SAMPLE_PLAT_VI_INIT(&stViConfig);
	if (s32Ret != CVI_SUCCESS) {
		printf("vi init failed. s32Ret: 0x%x !\n", s32Ret);
		return s32Ret;
	}

	SIZE_S stSizeIn, stSizeOut;

	stSizeIn.u32Width   = stSize.u32Width;
	stSizeIn.u32Height  = stSize.u32Height;
	stSizeOut.u32Width  = WIDTH;
	stSizeOut.u32Height = HEIGHT;
	s32Ret = SAMPLE_PLAT_VPSS_INIT(0, stSizeIn, stSizeOut);
	if (s32Ret != CVI_SUCCESS) {
		printf("vpss init failed. s32Ret: 0x%x !\n", s32Ret);
		CVI_BOOL  abChnEnable[VPSS_MAX_PHY_CHN_NUM] = {0};
		abChnEnable[0] = CVI_TRUE;
		SAMPLE_COMM_VPSS_Stop(0, abChnEnable);
		s32Ret = SAMPLE_PLAT_VPSS_INIT(0, stSizeIn, stSizeOut);
		if (s32Ret != CVI_SUCCESS) {
			printf("vpss init failed. s32Ret: 0x%x !\n", s32Ret);
			return s32Ret;
		}
	}

	s32Ret = SAMPLE_COMM_VI_Bind_VPSS(0, 0, 0);
	if (s32Ret != CVI_SUCCESS) {
		printf("vi bind vpss failed. s32Ret: 0x%x !\n", s32Ret);
		return s32Ret;
	}
	// SAMPLE_COMM_VI_Bind_VPSS会开启新的线程,过一会才执行完成
	// 利用这段时间,读取数据文件
	// usleep(500*1000);

	// 处理CTRL + C,不要设置的太早,会被mmf库覆盖掉设置
	// 不要绑定SIGALRM, solve.c要用

    struct sigaction sa;
    // 初始化信号动作结构体
    memset(&sa, 0, sizeof(sa));
    sa.sa_handler = fun_sig; // 设置信号处理函数
    sigemptyset(&sa.sa_mask);       // 不阻塞任何额外的信号
    sa.sa_flags = SA_RESTART;        // 可选标志，根据需求设置
    // 注册信号处理器,用signal注册的似乎不响应SIGTERM,改为用sigaction注册
	//signal(SIGINT, &fun_sig);	// ctrl-c
	//signal(SIGTERM, &fun_sig);  // kill
    if (sigaction(SIGTERM, &sa, NULL) == -1) {
        perror("sigaction");
        atomic_store(&exit_flag, 1);
    }
	if (sigaction(SIGINT, &sa, NULL) == -1) {
        perror("sigaction");
        atomic_store(&exit_flag, 1);
    }

	// 加载色块位置图
	if( read_cube_face_mask("cube_face.bin", cube_face_mask) != 0)
	{
		atomic_store(&exit_flag, 1);
	}
	// 加载二阶段魔方求解数据表
	if( load_lookup("lookup.dat") != 0)
	{
		atomic_store(&exit_flag, 1);
	}
	for(int i=0;i<10;i++){
		// 从通道获取处理完成的图像
		VIDEO_FRAME_INFO_S stVideoFrame;
		s32Ret = CVI_VPSS_GetChnFrame(0, 0, &stVideoFrame, 500);
		if (s32Ret != CVI_SUCCESS)
		{
			printf("CVI_VPSS_GetChnFrame for grp%d chn%d. s32Ret: 0x%x !\n", 0, 0, s32Ret);
		}
		CVI_VPSS_ReleaseChnFrame(0, 0, &stVideoFrame);
		//usleep(200*1000);
	}		


	printf("Ready. input 0 or 1: \n");
	if(uart_fd >= 0)
	{
		write(uart_fd, "\r\n", 2);
		write(uart_fd, "READY\r\n", 7);
	}
	
	while (atomic_load(&exit_flag) == 0)
	{
		char op;
		int has_date;
		if (uart_fd >= 0)
		{
			has_date = uart_getchar_with_timeout(uart_fd, &op);
			//write(uart_fd, &op, 1);
		}
		else
		{
			has_date = getchar_with_timeout(&op);
		}
		if(has_date != 1){
			//printf("wait date...\n");
			continue;
		}
		if (op == '0' || op == '1')
		{
			// 从通道获取一帧处理完成的图像
			VIDEO_FRAME_INFO_S stVideoFrame;
			s32Ret = CVI_VPSS_GetChnFrame(0, 0, &stVideoFrame, 500);
			if (s32Ret != CVI_SUCCESS)
			{
				printf("CVI_VPSS_GetChnFrame for grp%d chn%d. s32Ret: 0x%x !\n", 0, 0, s32Ret);
			}
			else
			{	
				if (uart_fd >= 0)
				{
					write(uart_fd, "OK\r\n", 4);
				}
				// stVideoFrame.stVFrame.enPixelFormat=19, PIXEL_FORMAT_NV21,
				// 数据是NV21格式的,可使用例程sample_venc -c jpg --pixel_format=3 -w 1280 -h 720 -i test.bin -o enc
				// 指令压缩为enc.jpg,方便查看.
				// pixel_format的解析程序在cvi_mmf_sdk/middleware/v2/sample/venc/src/sample_venc_lib.c vencMapPixelFormat
				if (op == '0')
				{
					copy_yuv(buffer0, &stVideoFrame);
					get_face_color(cube_face_mask, buffer0, &rgb_color[0]);
					//start = clock();
					//end = clock();
					//printf("get_face_color, time cost %fs\n", ((float)(end - start)) / CLOCKS_PER_SEC);
				}
				else if(op == '1')
				{
					copy_yuv(buffer1, &stVideoFrame);
					get_face_color(cube_face_mask, buffer1, &rgb_color[27]);
					// 生成日志文件
					FILE *fp = fopen("index.htm", "w");
					if (!fp) 
					{
						printf("Failed to open report.htm\n");
					}
					else
					{
						char solution[80];
						if(color_detect(rgb_color, fp, solution) == 0)
						{
							if (uart_fd >= 0)
							{
								write(uart_fd, "SOLUTION:", 9);
								write(uart_fd, solution, strlen(solution));
								write(uart_fd, "\r\n", 2);
							}
							printf("color_detect() success.\n");
						}
						else
						{
							if (uart_fd >= 0)
							{
								write(uart_fd, "FAIL\r\n", 6);
							}
							printf("color_detect() fail.\n");
						}
					}
					fclose(fp);
					// 执行比较慢,消耗时间0.3s x2左右,等求解魔方完成后再运行
					save_bmp("out0.bmp", buffer0);
					save_bmp("out1.bmp", buffer1);
				}
				CVI_VPSS_ReleaseChnFrame(0, 0, &stVideoFrame);
			}
		}
		// else if(op == '2')
		// {
		// 	solve_test("test_case.txt");
		// }
		else if(op == '\r' || op == '\n' )
		{
		 	write(uart_fd, "READY\r\n", 7);
		}
	}
	// 释放资源,如果不处理,下次启动程序会报错
	CVI_BOOL  abChnEnable[VPSS_MAX_PHY_CHN_NUM] = {0};
	abChnEnable[0] = CVI_TRUE;
	SAMPLE_COMM_VPSS_Stop(0, abChnEnable);
	SAMPLE_COMM_VI_DestroyIsp(&stViConfig);
	SAMPLE_COMM_VI_DestroyVi(&stViConfig);
	SAMPLE_COMM_SYS_Exit();

	free(buffer0);
	free(buffer1);
	free(cube_face_mask);
	free_lookup();
	if(uart_fd >= 0)
	{
		close(uart_fd);
	}
	return s32Ret;
}

